#ifndef MATHTOOL_H
#define MATHTOOL_H

#include"math.h"
#include <vector>
const static double PI = 3.1415926;

namespace MathTool {

    void QuaternionToRotation(std::vector<double>quat, double transform_res[4][4]);
    void EulerAnglesToQuaternion(std::vector<double> rpy, std::vector<double>& quat);
    void EulerAnglesToRotationByQuaternion(std::vector<double> rpy, double transform_res[4][4]);
    void RotationVectorToQuaternion(std::vector<double> rotation_vector, std::vector<double>& quat);
    void QuaternionToRotationVector(std::vector<double> quat, std::vector<double>& rotation_vec);
    void TransformToRotationVector(double transform[4][4], std::vector<double>& rotation_vec);
    void RotationVectorToTransform(std::vector<double> rotation_vector, double transform[4][4]);
    void MultiplyMatrix(double A[4][4], double B[4][4], double D[4][4]);
    void RotateAroundX(double theta, double rotation[4][4]);
    void RotateAroundY(double theta, double rotation[4][4]);
    void RotateAroundZ(double theta, double rotation[4][4]);
    void RotationTransformInverse4x4(double transmtx[4][4], double inverse[4][4]);

    // 返回数据顺序：XYZ (Roll: X, Pitch: Y, Yaw: Z)
    void RotationToEulerAngles(double transform_in[4][4], std::vector<double>& rpy);
    // YXZ
    void RotationToEulerAngles_YXZ(double transform_in[4][4], std::vector<double>& rpy);

    double DegToRadius(double theta);
    double RadiusToDeg(double theta);

    void IdentifyMatrix4x4(double matrix[4][4]);

    // 归一化处理 [YINGSHEN ZHAO 2023-06-13]
    void UnifyVector(std::vector<double>& v);

    // 判断是否为零数据
    bool IsZeroVector(std::vector<double>& v, int size);

    // 旋转矩阵转轴角
    void RotationToAxisAngle(double transform[4][4], std::vector<double>& axis, double& angle);

    // 计算两个向量之间的角度
    double ComputeAnglesByTwoVectors(std::vector<double> v1, std::vector<double> v2);

};

#endif // MATHTOOL_H
